//
//  Sector.cpp
//  trail-map-editor
//
//  Created by Tino Giorgio on 12/3/13.
//
//

#include "Sector.h"

#include <plib/ssgLocal.h>

#include "ui.h"

#include "Defines.h"

#define FILE_ROW_STRING_LENGTH 80
#define SECTOR_GRID_STEP 25 // meters
#define MIN_HEIGHT 0.0 // Instead of -9999

#define isOutOfBounds(x)(x < MIN_HEIGHT) // Who knows if this will change in the future...

typedef struct
{
    int x;
    int y;
} iCoord;

typedef enum
{
    UNKNOWN = -1,
    NORTH,
    EAST,
    SOUTH,
    WEST
} eDirection;

// ----------------------------------------------------------------
// Static variables
// ----------------------------------------------------------------
bool Sector::smWireframeEnabled = false;
sgFloat Sector::smMapOffsetU = 0.5f;
sgFloat Sector::smMapOffsetV = 1.5f;

// ----------------------------------------------------------------
// Private constructor (use factory methods!)
// ----------------------------------------------------------------
Sector::Sector(const char* name, int east, int north, int r, int c, int gridStep, int gridThinFactor)
: mState(new ssgSimpleState())
{
	mName = ulStrDup(name);
	mEast = east;
	mNorth = north;
	mRows = r;
	mColumns = c;
	mGridStep = gridStep;
	mGridThinFactor = gridThinFactor;
	
	
    mState -> disable           ( GL_TEXTURE_2D ) ;
    mState -> enable            ( GL_COLOR_MATERIAL ) ;
    mState -> setShadeModel     ( GL_SMOOTH ) ;
    mState -> enable            ( GL_CULL_FACE ) ;
    mState -> enable            ( GL_BLEND ) ;
    mState -> enable            ( GL_LIGHTING ) ;
    mState -> setColourMaterial ( GL_AMBIENT_AND_DIFFUSE ) ;
    mState -> setMaterial       ( GL_DIFFUSE, 0.1, 0.1, 0.1, 1 ) ;
    mState -> setMaterial       ( GL_EMISSION, 0, 0, 0, 1 ) ;
    mState -> setMaterial       ( GL_SPECULAR, 0, 0, 0, 1 ) ;
    mState -> setShininess      ( 0 ) ;
}

Sector::~Sector()
{    
    mState = NULL;
    
    SAFE_DELETE_STRING(mName);
}

// ----------------------------------------------------------------
// Factory static methods
// ----------------------------------------------------------------
//Sector* Sector::loadFromDEM(const char* filename, bool centerOnOrigin)
//{
//	sgFloat** heightMap;
//	
//    FILE* f;
//    char fileRow[FILE_ROW_STRING_LENGTH];
//    char ch1,ch2;
//    char name[FILE_ROW_STRING_LENGTH];
//    int c = 0;
//    int r = 0;
//    int east = 0;
//    int north = 0;
//    int i = 0;
//    int j = 0;
//    float ah = 0;
//    
//    /* LOADER */
//    if((f = fopen(filename,"r")) != NULL)
//    {
//		/* jump beyond header */
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//        sscanf(fileRow,"%s",name);
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//        
//		/* read the number of columns */
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//		sscanf(fileRow,"%c%c%d",&ch1,&ch2,&c);
//        
//		/* read the number of rows */
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//		sscanf(fileRow,"%c%c%d",&ch1,&ch2,&r);
//        
//		/* read the x_start_coord (and  stores it in x)  */
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//		sscanf(fileRow,"%d",&east);
//        
//		/* read the y_start_coord (and  stores it in y) */
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//		sscanf(fileRow,"%d",&north);
//        
//		/* read the average height */
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//		sscanf(fileRow,"%f",&ah);
//		
//		/* allocate a height map of r * c */
//		heightMap = new sgFloat*[r];
//		heightMap[0] = new sgFloat[r*c];
//        
//        for(int i=1; i<r; i++)
//        {
//            heightMap[i] = &heightMap[0][i*c];
//        }
//        
//		/* jump over another line */
//		fgets(fileRow,FILE_ROW_STRING_LENGTH,f);
//        
//		/* read the lines containing the height values */
//		/* This matrix is filled per columns, from the bottom to the top,
//         and from the left to the right. So data in memory have the 'same'
//         position (each other) than in the real world...*/
//		while(fgets(fileRow,FILE_ROW_STRING_LENGTH,f) != NULL)
//        {
//			if(j < r)
//            {
//				sscanf(fileRow,"%f",&(heightMap[r-j-1][i]));
//				j++;
//            }
//			else
//            {
//				j = 0;
//				i += 1;
//				sscanf(fileRow,"%f",&(heightMap[r-j-1][i]));
//				j++;
//            }
//        }
//        
//		/* close the file stream  */
//		fclose(f);
//    }
//	else
//    {
//		printf("Cannot open .DEM file %s for reading.\n",filename);
//		return NULL;
//    }
//    
//	sgVec3** normalMap = _makeNormalMap(heightMap, r, c, SECTOR_GRID_STEP);
//	
//    ssgVtxTable* vertexTable = _createVertexTable(heightMap, normalMap, r, c, SECTOR_GRID_STEP);
//	
//	ssgStripify(vertexTable);
//	vertexTable->makeDList();
//	
//	ssgSimpleState* state = new ssgSimpleState();
//	
//	char* texturePath = uiMakeResourcePath("019160.png");//uiStrCat();
//	
//    if(texturePath)
//    {
//        ssgTexture* tex = new ssgTexture(texturePath);
//
//        state -> setTexture		   ( tex );
//        state -> enable            ( GL_TEXTURE_2D ) ;
//    }
//    else
//    {
//        state -> disable( GL_TEXTURE_2D);
//    }
//    
//    DELETE_STRING(texturePath);
//    
//    state -> enable            ( GL_COLOR_MATERIAL ) ;
//    state -> setShadeModel     ( GL_SMOOTH ) ;
//    state -> enable            ( GL_CULL_FACE ) ;
//    state -> enable            ( GL_BLEND ) ;
//    state -> enable            ( GL_LIGHTING ) ;
//    state -> setColourMaterial ( GL_AMBIENT_AND_DIFFUSE ) ;
//    state -> setMaterial       ( GL_DIFFUSE, 0, 0, 0, 1 ) ;
//    state -> setMaterial       ( GL_EMISSION, 0, 0, 0, 1 ) ;
//    state -> setMaterial       ( GL_SPECULAR, 0, 0, 0, 1 ) ;
//    state -> setShininess      ( 0 ) ;
//	
//	vertexTable->setState(state);
//	
//	//
//	// Cleanup the memory
//	//
//	SAFE_DELETE_MATRIX(heightMap);
//	SAFE_DELETE_MATRIX(normalMap);
//	
//	Sector* sector = new Sector(name,east*10.0f,north*10.0f,r,c,SECTOR_GRID_STEP,1);
//	sector->addKid(vertexTable);
//	
//	if(centerOnOrigin)
//		sector->centerOnOrigin();
//	
//	return sector;
//}
Sector* Sector::loadFromDEM(const char* filename, bool centerOnOrigin)
{
	sgFloat** heightMap;
	
    char* id;
    int c = 0;
    int r = 0;
    int east = 0;
    int north = 0;
    int i = 0;
    int j = 0;
    int units = 0;
    float step;
    
    /* LOADER */
	FILE *f = fopen ( filename, "rb" ) ;
	
	if ( f == NULL )
	{
		perror ( filename ) ;
		ulSetError ( UL_FATAL,
					"loadFromDEM: Failed to open '%s' for reading.", filename ) ;
		return NULL;
	}
    
    printf("\n\nloading : %s\n\n",filename);
    
    _ssgReadString(f, &id);
    _ssgReadInt( f, &r );
    _ssgReadInt( f, &c );
    _ssgReadFloat( f, &step );
    _ssgReadInt( f, &units );
    _ssgReadInt( f, &east);
    _ssgReadInt( f, &north );
    
    /* allocate a height map of r * c */
    heightMap = new sgFloat*[r];
    heightMap[0] = new sgFloat[r*c];
    
    for(int i=1; i<r; i++)
    {
        heightMap[i] = &heightMap[0][i*c];
    }
    
    /* read the lines containing the height values */
    /* This matrix is filled per columns, from the bottom to the top,
     and from the left to the right. So data in memory have the 'same'
     position (each other) than in the real world...*/
    int n = r*c;
    while(n>0)
    {
        if(j < r)
        {
            _ssgReadFloat(f,&(heightMap[r-j-1][i]));
            j++;
        }
        else
        {
            j = 0;
            i += 1;
            _ssgReadFloat(f,&(heightMap[r-j-1][i]));
            j++;
        }
        
        n--;
    }
    
    /* close the file stream  */
    fclose(f);
    
	sgVec3** normalMap = _makeNormalMap(heightMap, r, c, SECTOR_GRID_STEP);
	
    //
    // Create and setup the vertexTable for the terrain 'top'
    //
    ssgVtxTable* vtTerrain = _createTerrain(heightMap, normalMap, r, c, SECTOR_GRID_STEP);
	
	ssgStripify(vtTerrain);
	vtTerrain->makeDList();
	
	ssgSimpleState* stTerrain = new ssgSimpleState();
	
    char textureName[80];
    sprintf(textureName, "DTM/%s.png",id);
	char* texturePath = uiMakeResourcePath(textureName);//uiStrCat();
	
    if(texturePath)
    {
        ssgTexture* tex = new ssgTexture(texturePath);
        
        stTerrain -> setTexture		   ( tex );
        stTerrain -> enable            ( GL_TEXTURE_2D ) ;
    }
    else
    {
        stTerrain -> disable( GL_TEXTURE_2D);
    }
    
    DELETE_STRING(texturePath);
    
    stTerrain -> enable            ( GL_COLOR_MATERIAL ) ;
    stTerrain -> setShadeModel     ( GL_SMOOTH ) ;
    stTerrain -> enable           ( GL_CULL_FACE ) ; // NOTE : enable it to avoid drawing back facing triangles
    stTerrain -> enable            ( GL_BLEND ) ;
    stTerrain -> enable            ( GL_LIGHTING ) ;
    stTerrain -> setColourMaterial ( GL_AMBIENT_AND_DIFFUSE ) ;
    stTerrain -> setMaterial       ( GL_DIFFUSE, 0, 1, 0, 1 ) ;
    stTerrain -> setMaterial       ( GL_EMISSION, 0, 0, 0, 1 ) ;
    stTerrain -> setMaterial       ( GL_SPECULAR, 0, 0, 0, 1 ) ;
    stTerrain -> setShininess      ( 0 ) ;
	
	vtTerrain->setState(stTerrain);
    
    //
    // Create and setup the vertex table for the terrain 'box'
    //
    ssgVtxTable* vtBox = _createBox(heightMap, r, c, SECTOR_GRID_STEP);

	ssgStripify(vtBox);
	vtBox->makeDList();
	
	ssgSimpleState* stBox = new ssgSimpleState();
	
	texturePath = uiMakeResourcePath("box.png");//uiStrCat();
	
    if(texturePath)
    {
        ssgTexture* tex = new ssgTexture(texturePath);
        
        stBox -> setTexture		   ( tex );
        stBox -> enable            ( GL_TEXTURE_2D ) ;
    }
    else
    {
        stBox -> disable( GL_TEXTURE_2D);
    }
    
    DELETE_STRING(texturePath);
    
    stBox -> enable            ( GL_COLOR_MATERIAL ) ;
    stBox -> setShadeModel     ( GL_SMOOTH ) ;
    stBox -> enable           ( GL_CULL_FACE ) ; // NOTE : enable it to avoid drawing back facing triangles
    stBox -> enable            ( GL_BLEND ) ;
    stBox -> enable            ( GL_LIGHTING ) ;
    stBox -> setColourMaterial ( GL_AMBIENT_AND_DIFFUSE ) ;
    stBox -> setMaterial       ( GL_DIFFUSE, 0, 0, 0, 1 ) ;
    stBox -> setMaterial       ( GL_EMISSION, 0, 0, 0, 1 ) ;
    stBox -> setMaterial       ( GL_SPECULAR, 0, 0, 0, 1 ) ;
    stBox -> setShininess      ( 0 ) ;
	
	vtBox->setState(stBox);
	
	//
	// Cleanup the memory
	//
	SAFE_DELETE_MATRIX(heightMap);
	SAFE_DELETE_MATRIX(normalMap);
	
	Sector* sector = new Sector(id,east*10.0f,north*10.0f,r,c,SECTOR_GRID_STEP,1);
	sector->addKid(vtTerrain);
    sector->addKid(vtBox);
	
	if(centerOnOrigin)
		sector->centerOnOrigin();
	
	return sector;
}

Sector* Sector::loadFromBinary(const char* filename, bool centerOnOrigin)
{
	char* id;
    int c = 0;
    int r = 0;
    int east = 0;
    int north = 0;
	
	FILE *fd = fopen ( filename, "rb" ) ;
	
	if ( fd == NULL )
	{
		perror ( filename ) ;
		ulSetError ( UL_WARNING, 
					"loadFromMAP: Failed to open '%s' for reading.", filename ) ;
		return NULL;
	}
	
	_ssgReadString( fd, &id );
	_ssgReadInt( fd, &r );
	_ssgReadInt( fd, &c );
	_ssgReadInt( fd, &east );
	_ssgReadInt( fd, &north );
	
	//
	// Allocate normal map
	//
	sgVec3** normalMap = new sgVec3*[r];
	normalMap[0] = new sgVec3[r*c];
	
	for(int i=1; i < r; i++)
	{
		normalMap[i] = &normalMap[0][i*c];
	}
	
	//
	// Allocate height map
	//
	sgFloat** heightMap = new sgFloat*[r];
	heightMap[0] = new sgFloat[r*c];
	
	for(int i=1; i<r; i++)
	{
		heightMap[i] = &heightMap[0][i*c];
	}
	
	//
	// Save height map
	//
	for(int i = 0; i < r; i++)
	{
		for(int j = 0; j < c; j++)
		{
			_ssgReadFloat(fd, &heightMap[i][j]);
			_ssgReadFloat(fd, &normalMap[i][j][0]);
			_ssgReadFloat(fd, &normalMap[i][j][1]);
			_ssgReadFloat(fd, &normalMap[i][j][2]);
		}
	}
	
	fclose ( fd );
	
	ssgVtxTable* vertexTable =  _createTerrain(heightMap, normalMap, r, c, SECTOR_GRID_STEP);
	
	ssgStripify(vertexTable);
	vertexTable->makeDList();
	
	//
	// Cleanup the memory
	//
	delete[] heightMap[0];
	delete[] heightMap;
	heightMap = NULL;
	
	delete[] normalMap[0];
	delete[] normalMap;
	normalMap = NULL;
	
	Sector* sector = new Sector(id,east,north,r,c,SECTOR_GRID_STEP,1);
	sector->addKid(vertexTable);
    
	if(centerOnOrigin)
		sector->centerOnOrigin();
	
	return sector;
}

sgVec3** Sector::_makeNormalMap(sgFloat** heightMap, int r, int c, float gridStep)
{
	sgVec3** normalMap;
	
	/* allocate a height map of r * c */
	normalMap = new sgVec3*[r];
	normalMap[0] = new sgVec3[r*c];
	
	for(int i=1; i < r; i++)
	{
		normalMap[i] = &normalMap[0][i*c];
	}
	
	for(int i = 0; i < r; i++)
    {
        for(int j = 0; j < c; j++)
        {
			sgZeroVec3(normalMap[i][j]);
		}
	}
	
	for(int i = 0; i < r-1; i++)
    {
        for(int j = 0; j < c-1; j++)
        {
			sgVec3 v0,v1,v2,v3;
			
			float h0 = heightMap[i][j];
			float h1 = heightMap[i+1][j];
			float h2 = heightMap[i+1][j+1];
			float h3 = heightMap[i][j+1];
			
			// Skip the triangles with one or more vertex at -1000 (outside the map) or the normal won't be correct.
			if(h0 <= 0 || h1 <= 0 || h2 <= 0 || h3 <= 0)
				continue;
			
			sgSetVec3(v0,gridStep*j,       gridStep*(r - 1 - i),  h0);
            sgSetVec3(v1,gridStep*j,       gridStep*(r - 2 - i),  h1);
            sgSetVec3(v2,gridStep*(j+1),   gridStep*(r - 2 - i),  h2);
            sgSetVec3(v3,gridStep*(j+1),   gridStep*(r - 1 - i),  h3);
			
			sgVec3 n0, n1;
			
			sgMakeNormal(n0, v0, v1, v2);
            sgMakeNormal(n1, v2, v3, v0);
			
			sgAddVec3(normalMap[i][j],n0);
			sgAddVec3(normalMap[i+1][j],n0);
			sgAddVec3(normalMap[i+1][j+1],n0);
			sgAddVec3(normalMap[i+1][j+1],n1);
			sgAddVec3(normalMap[i][j+1],n1);
			sgAddVec3(normalMap[i][j],n1);
		}
	}
	
	for(int i = 0; i < r; i++)
    {
        for(int j = 0; j < c; j++)
        {
			sgNormaliseVec3(normalMap[i][j]);
		}
	}
	
	return normalMap;
}	

ssgVtxTable* Sector::_createTerrain(sgFloat** heightMap, sgVec3** normalMap, int r, int c, float gridStep)
{    
    int nVertices = (c-1)*(r-1)*6;
    
    if(nVertices < 6)
        return NULL;
    
    //
    // Instantiate the arrays
    //
    ssgVertexArray *vertexArray   = new ssgVertexArray(nVertices);
    ssgNormalArray *normalArray   = new ssgNormalArray(nVertices);
    ssgTexCoordArray *texCoordArray = new ssgTexCoordArray(nVertices);
    ssgColourArray *colourArray   = new ssgColourArray(nVertices);
    
    for(int i = 0; i < r-1; i++)
    {
        for(int j = 0; j < c-1; j++)
        {
            //
            // Fill them with data for GL_TRIANGLES
            //
            sgVec3 v0,v1,v2,v3;
            sgVec3 n0,n1,n2,n3;
            sgVec2 t0,t1,t2,t3;
            sgVec4 c0;//,c1,c2,c3;
            
            // Vertices
            
            // [0,0]
            // v0 --- v3
            //  |     |
            //  |     |
            // v1 --- v2
            
            sgSetVec3(v0,gridStep*j,       gridStep*(r - 1 - i),  heightMap[i][j]);
            sgSetVec3(v1,gridStep*j,       gridStep*(r - 2 - i),  heightMap[i+1][j]);
            sgSetVec3(v2,gridStep*(j+1),   gridStep*(r - 2 - i),  heightMap[i+1][j+1]);
            sgSetVec3(v3,gridStep*(j+1),   gridStep*(r - 1 - i),  heightMap[i][j+1]);
            
            //
            // Skip triangles with at least one vertex with z < 0 (it's outside of the map!)
            //
            if(v0[SG_Z] < 0 || v1[SG_Z] < 0 || v2[SG_Z] < 0 || v3[SG_Z] < 0)
                continue;
            
            vertexArray->add(v0);
            vertexArray->add(v1);
            vertexArray->add(v2);
            vertexArray->add(v2);
            vertexArray->add(v3);
            vertexArray->add(v0);
            
            // Normals
			sgCopyVec3(n0,normalMap[i][j]);
            sgCopyVec3(n1,normalMap[i+1][j]);
			sgCopyVec3(n2,normalMap[i+1][j+1]);
			sgCopyVec3(n3,normalMap[i][j+1]);
			
            normalArray->add(n0);
            normalArray->add(n1);
            normalArray->add(n2);
            normalArray->add(n2);
            normalArray->add(n3);
            normalArray->add(n0);
            
            // Tex Coords
            float u = (float)j+smMapOffsetU; // TBD
            float v = (float)i+smMapOffsetV; // TBD
            
            sgSetVec2(t0,u/(float)c,((v)/(float)r));
            sgSetVec2(t1,u/(float)c,((v+1)/(float)r));
            sgSetVec2(t2,(u+1)/(float)c,((v+1)/(float)r));
            sgSetVec2(t3,(u+1.0f)/(float)c,(v/(float)r));
            
            texCoordArray->add(t0);
            texCoordArray->add(t1);
            texCoordArray->add(t2);
            texCoordArray->add(t2);
            texCoordArray->add(t3);
            texCoordArray->add(t0);
            
            // Colours
            sgSetVec4(c0, 0.8f, 0.8f, 0.8f, 1.0f);
            
            colourArray->add(c0);
            colourArray->add(c0);
			colourArray->add(c0);
            colourArray->add(c0);
            colourArray->add(c0);
			colourArray->add(c0);
        }
    }
    
    //
    // Return the vertex table
    //
    ssgVtxTable* vertexTable = new ssgVtxTable(GL_TRIANGLES,
                                   vertexArray,
                                   normalArray,
                                   texCoordArray,
                                   colourArray);
	
	vertexTable->setCallback(SSG_CALLBACK_POSTDRAW,_callbackPostDraw);
	
	return vertexTable;
}

bool Sector::_isBorder(sgFloat** heightMap, bool** visited, int r, int c, int x, int y)
{
    if(isOutOfBounds(heightMap[y][x]) || visited[y][x])
        return false;
    
    // Top left
    for(int i = y-1; i < y+2; i++ )
    {
        for(int j = x-1; j < x+2; j++)
        {
            if((i >= 0) &&
               (j >= 0) &&
               (i < r) &&
               (j < c) &&
               (i != y || j != x) &&
               isOutOfBounds(heightMap[i][j]) &&
               !visited[i][j])
            {
                return true;
            }
        }
    }
    
    return false;
}

ssgVtxTable* Sector::_createBox(sgFloat** heightMap, int r, int c, float gridStep)
{
    int nVertices = (c+r)*2*6;
    
    if(nVertices < 6)
        return NULL;
    
    //printf("[%d %d] : %d\n", r, c, nVertices);
    
    //
    // Instantiate the arrays
    //
    ssgVertexArray *vertexArray   = new ssgVertexArray(nVertices);
    ssgNormalArray *normalArray   = new ssgNormalArray(nVertices);
    ssgTexCoordArray *texCoordArray = new ssgTexCoordArray(nVertices);
    ssgColourArray *colourArray   = new ssgColourArray(nVertices);
    
    iCoord current = {0 , 0};
    
    iCoord start = {-1, -1};
    
    //
    // Find the borders of the map to create the boarder box. A point is considered to be part of the border
    // if at least one of the adjacent points (top left bottom right) height is < 0 (DEM files have values of -9999 usually)
    //
    // NOTE : this is assuming that there is only one 'island' and no holes or crazy configurations! Let's keep it simple...
    //
    
    //
    // STEP 1 : find the strating point
    //
    bool found = false;
    
//    for(int i = 0; i < r; i++)
//    {
//        for(int j = 0; j < c; j++)
//        {
//            printf("i,j is [%d,%d] is %f : %d\n",i,j,heightMap[i][j],isOutOfBounds(heightMap[i][j]));
//        }
//    }
    
    for(int i = 0; (i < r) && !found; i++)
    {
        for(int j = 0; (j < c) && !found; j++)
        {
            //printf("i,j is [%d,%d] is %f : %d\n",i,j,heightMap[i][j],isOutOfBounds(heightMap[i][j]));
            
            if(!isOutOfBounds(heightMap[i][j]))
            {
                start.x = j;
                start.y = i;
                found = true;
            }
        }
    }
    
    //
    // Completely empty map. Very weird, should NEVER happen!
    //
    if(!found)
        return NULL;
    
    bool** visited = new bool*[r];
    visited[0] = new bool[r*c]();
    
    for(int i=1; i<r; i++)
    {
        visited[i] = &visited[0][i*c];
    }
   
    //printf("Start : [%d,%d] %p\n",start.x,start.y,vertexArray);
    
    visited[start.y][start.x] = true;
    
    //
    // STEP 1 : from the starting point, start following the boundary (top left bottom right) always excluding the direction we are coming from to avoid loops. When the current position is equal to the starting position, we consider the box closed. NOTE : again, this works for convex trrains with one island only and no holes. Which is correct for all the DEM files I have ever seen.
    //
    current = start;
    
    eDirection direction = UNKNOWN;
    
    bool done = false;
    
    do
    {
        iCoord next = current;
        
        //printf("{%d,%d}\n",next.x,next.y);
        
        bool last = false;
        
        // Check NORTH
        if((direction != SOUTH) &&
           (current.y-1 >= 0) &&
           _isBorder(heightMap, visited, r, c, current.x , current.y-1))
        {
            next.x = current.x;
            next.y = current.y - 1;
            
            // next is north!
            direction = NORTH;
        }
        else if((direction != WEST) &&
                (current.x+1 < c) &&
                _isBorder(heightMap, visited, r, c, current.x+1, current.y))
        {
            // next is east!
            next.x = current.x + 1;
            next.y = current.y;
            
            direction = EAST;
        }
        else if((direction != NORTH) &&
                (current.y+1 < r) &&
                _isBorder(heightMap, visited, r, c, current.x, current.y+1))
        {
            // next is south!
            next.x = current.x;
            next.y = current.y+1;
            
            direction = SOUTH;
        }
        else if((direction != EAST) &&
                (current.x-1 >= 0) &&
                _isBorder(heightMap, visited, r, c, current.x-1, current.y))
        {
            // next is west!
            next.x = current.x - 1;
            next.y = current.y;
            
            direction = WEST;
        }
        else
        {            
            next = start;
            last = true;
        }
        
        //printf("[%d,%d] %d\n",next.x,next.y,direction);
        
        if(direction != UNKNOWN) // should never happen
        {
            // Get the height at current and next point point
            sgFloat h0 = heightMap[current.y][current.x];
            sgFloat h1 = heightMap[next.y][next.x];
            
            //
            // Fill them with data for GL_TRIANGLES
            //
            sgVec3 v0,v1,v2,v3;
            sgVec3 n0;
            sgVec2 t0,t1,t2,t3;
            sgVec4 c0;//,c1,c2,c3;
            
            // Vertices
            
            // [0,0]
            // v0 --- v3
            //  |     |
            //  |     |
            // v1 --- v2
            
            sgSetVec3(v0,gridStep*current.x,    gridStep* (r - current.y - 1),     MIN_HEIGHT);
            sgSetVec3(v1,gridStep*current.x,    gridStep* (r - current.y - 1),     h0);
            sgSetVec3(v2,gridStep*next.x,       gridStep* (r - next.y - 1),        h1);
            sgSetVec3(v3,gridStep*next.x,       gridStep* (r - next.y - 1),        MIN_HEIGHT);
            
            vertexArray->add(v0);
            vertexArray->add(v1);
            vertexArray->add(v2);
            vertexArray->add(v2);
            vertexArray->add(v3);
            vertexArray->add(v0);
            
            // Normals
            // Same normal for all vertices, can only be one out of four depending on the side.
            
            switch (direction) {
                case NORTH:
                    sgSetVec3(n0, -1, 0, 0); // Pointing towards 'west';
                    break;
                case EAST:
                    sgSetVec3(n0, 0, 1, 0); // Pointing towards 'north';
                    break;
                case SOUTH:
                    sgSetVec3(n0, 1, 0, 0); // Pointing towards 'east';
                    break;
                case WEST:
                    sgSetVec3(n0, 0, -1, 0); // Pointing towards 'south';
                    break;
                default:
                    // We won't even get to this point if that's the case!
                    break;
            }
            
            normalArray->add(n0);
            normalArray->add(n0);
            normalArray->add(n0);
            normalArray->add(n0);
            normalArray->add(n0);
            normalArray->add(n0);
            
            // Tex Coords
            float u1 = h0 / gridStep; // Texture is squared and will wrap vertically
            float u2 = h1 / gridStep;
            
            sgSetVec2(t0,1.0,0.0);
            sgSetVec2(t1,1.0,u1);
            sgSetVec2(t2,0.0,u2);
            sgSetVec2(t3,0.0,0.0);
            
            texCoordArray->add(t0);
            texCoordArray->add(t1);
            texCoordArray->add(t2);
            texCoordArray->add(t2);
            texCoordArray->add(t3);
            texCoordArray->add(t0);
            
            // Colours
            sgSetVec4(c0, 1.0f, 1.0f, 1.0f, 1.0f);
            
            colourArray->add(c0);
            colourArray->add(c0);
            colourArray->add(c0);
            colourArray->add(c0);
            colourArray->add(c0);
            colourArray->add(c0);
        }
        
        if(last)
        {
            done = true;
        }
        
        current = next;
    }
    while(!done);

    //
    // Return the vertex table
    //
    ssgVtxTable* vertexTable = new ssgVtxTable(GL_TRIANGLES,
                                               vertexArray,
                                               normalArray,
                                               texCoordArray,
                                               colourArray);
	
	vertexTable->setCallback(SSG_CALLBACK_POSTDRAW,_callbackPostDraw);
	
	return vertexTable;
}

ssgVtxTable* Sector::_createBottom(sgFloat** heightMap, int r, int c, float gridStep)
{
    int nVertices = (c-1)*(r-1)*6;
    
    if(nVertices < 6)
        return NULL;
    
    //
    // Instantiate the arrays
    //
    ssgVertexArray *vertexArray   = new ssgVertexArray(nVertices);
    ssgNormalArray *normalArray   = new ssgNormalArray(nVertices);
    ssgTexCoordArray *texCoordArray = new ssgTexCoordArray(nVertices);
    ssgColourArray *colourArray   = new ssgColourArray(nVertices);
    
//    for(int i = 0; i < r-1; i++)
//    {
//        for(int j = 0; j < c-1; j++)
//        {
//            //
//            // Fill them with data for GL_TRIANGLES
//            //
//            sgVec3 v0,v1,v2,v3;
//            sgVec3 n0,n1,n2,n3;
//            sgVec2 t0,t1,t2,t3;
//            sgVec4 c0;//,c1,c2,c3;
//            
//            // Vertices
//            
//            // [0,0]
//            // v0 --- v3
//            //  |     |
//            //  |     |
//            // v1 --- v2
//            
//            sgSetVec3(v0,gridStep*j,       gridStep*(r - 1 - i),  heightMap[i][j]);
//            sgSetVec3(v1,gridStep*j,       gridStep*(r - 2 - i),  heightMap[i+1][j]);
//            sgSetVec3(v2,gridStep*(j+1),   gridStep*(r - 2 - i),  heightMap[i+1][j+1]);
//            sgSetVec3(v3,gridStep*(j+1),   gridStep*(r - 1 - i),  heightMap[i][j+1]);
//            
//            //
//            // Skip triangles with at least one vertex with z < 0 (it's outside of the map!)
//            //
//            if(v0[SG_Z] < 0 || v1[SG_Z] < 0 || v2[SG_Z] < 0 || v3[SG_Z] < 0)
//                continue;
//            
//            vertexArray->add(v0);
//            vertexArray->add(v1);
//            vertexArray->add(v2);
//            vertexArray->add(v2);
//            vertexArray->add(v3);
//            vertexArray->add(v0);
//            
//            // Normals
//			sgCopyVec3(n0,normalMap[i][j]);
//            sgCopyVec3(n1,normalMap[i+1][j]);
//			sgCopyVec3(n2,normalMap[i+1][j+1]);
//			sgCopyVec3(n3,normalMap[i][j+1]);
//			
//            normalArray->add(n0);
//            normalArray->add(n1);
//            normalArray->add(n2);
//            normalArray->add(n2);
//            normalArray->add(n3);
//            normalArray->add(n0);
//            
//            // Tex Coords
//            float u = (float)j;
//            float v = (float)i;
//            
//            sgSetVec2(t0,u/(float)c,((v)/(float)r));
//            sgSetVec2(t1,u/(float)c,((v+1)/(float)r));
//            sgSetVec2(t2,(u+1)/(float)c,((v+1)/(float)r));
//            sgSetVec2(t3,(u+1.0f)/(float)c,(v/(float)r));
//            
//            texCoordArray->add(t0);
//            texCoordArray->add(t1);
//            texCoordArray->add(t2);
//            texCoordArray->add(t2);
//            texCoordArray->add(t3);
//            texCoordArray->add(t0);
//            
//            // Colours
//            sgSetVec4(c0, 0.8f, 0.8f, 0.8f, 1.0f);
//            
//            colourArray->add(c0);
//            colourArray->add(c0);
//			colourArray->add(c0);
//            colourArray->add(c0);
//            colourArray->add(c0);
//			colourArray->add(c0);
//        }
//    }
    
    //
    // Return the vertex table
    //
    ssgVtxTable* vertexTable = new ssgVtxTable(GL_TRIANGLES,
                                               vertexArray,
                                               normalArray,
                                               texCoordArray,
                                               colourArray);
	
	vertexTable->setCallback(SSG_CALLBACK_POSTDRAW,_callbackPostDraw);
	
	return vertexTable;
}

bool Sector::convertDEMToBinary(const char* input, const char* output)
{
//	FILE *fd = fopen ( output, "wb" ) ;
//	
//	if ( fd == NULL )
//	{
//		perror ( filename ) ;
//		ulSetError ( UL_WARNING, 
//					"saveToMAP: Failed to open '%s' for writing.", filename ) ;
//		return FALSE ;
//	}
//	
//	_ssgWriteString( fd, sector->mName );
//	_ssgWriteInt( fd, sector->mRows );
//	_ssgWriteInt( fd, sector->mColumns );
//	_ssgWriteInt( fd, sector->mGridStep );
//	_ssgWriteInt( fd, sector->mGridThinFactor );
//	_ssgWriteInt( fd, sector->mEast );
//	_ssgWriteInt( fd, sector->mNorth );
//	
//	//
//	// Save height map
//	//
//	for(int i = 0; i < mRows; i++)
//	{
//		for(int j = 0; j < mColumns; j++)
//		{
//			_ssgWriteFloat(fd, mHeightMap[i][j]);
//			_ssgWriteVec3(fd, mNormalMap[i][j]);
//		}
//	}
//	
//	fclose ( fd ) ;  
	
	return TRUE ;
}
 
// ----------------------------------------------------------------
// Debug methods
// ----------------------------------------------------------------
void Sector::print() const
{
    printf("\n");
    printf("Name  : %s\n", this->mName ? this->mName : "");
    printf("Size  : [%dx%d]\n", this->mRows, this->mColumns);
    printf("East  : %d\n", this->mEast);
    printf("North : %d\n", this->mNorth);
    // printf("Data  :\n");
    
	//this->print(stdout,"\t",4);
}

// ----------------------------------------------------------------
// Draw callbacks
// ----------------------------------------------------------------
static sgVec4 wire_colour = { 1.0f, .0f, .0f, 1.0f } ;
int Sector::_callbackPostDraw(ssgEntity* e)
{
    if ( smWireframeEnabled &&  e->isAKindOf(ssgTypeLeaf()) )
    {
        ssgLeaf* leaf = (ssgLeaf*) e ;
        leaf -> drawHighlight ( wire_colour ) ;
    }
	
    return _callbackDrawNormals(e) ;
}

static sgVec4 normal_colour = { 1.0f, 0.0f, 1.0f, 1.0f };
int Sector::_callbackDrawNormals(ssgEntity* e)
{
	if ( smWireframeEnabled && e->isAKindOf(ssgTypeLeaf()))
	{
		ssgLeaf* leaf = (ssgLeaf*) e;
		
		_ssgForceLineState () ;
		
		int num_vertices  = leaf->getNumVertices  () ;
		
		glColor4fv ( normal_colour ) ;
		glLineWidth ( 1.0f ) ;
		
		for(int i = 0; i < num_vertices; i++)
		{			
			sgVec3 *nx = (sgVec3 *) leaf->getNormal(i);
			sgVec3 *vx = (sgVec3 *) leaf->getVertex(i);
			
			float x0 = vx[0][0] ;
			float y0 = vx[0][1] ;
			float z0 = vx[0][2] ;
			float x1 = x0 + SECTOR_GRID_STEP * nx[0][0] ;
			float y1 = y0 + SECTOR_GRID_STEP * nx[0][1] ;
			float z1 = z0 + SECTOR_GRID_STEP * nx[0][2] ;
			
			sgVec3 v[2];
			sgSetVec3(v[0], x0, y0, z0);
			sgSetVec3(v[1], x1, y1, z1);
			
			
			//sgAddVec3 ( n1, x-,y,z ) ;
			glBegin ( GL_LINES ) ;
			glVertex3fv ( v[0] ) ;
			glVertex3fv ( v[1] ) ;
			glEnd () ;
		}
		
		glLineWidth ( 1.0f ) ;
		glEnable ( GL_DEPTH_TEST ) ;
	}
	
	return 1;
}
